#pragma once

#include <sys/socket.h>
#include <sys/select.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <cstring>
#include <unistd.h>
#include <fcntl.h>
#include <vector>
#include <cmath>
#include <stdio.h>
#include <thread>
#include <boost/asio.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <filesystem>
#include <atomic>
#include <deque>



namespace MotorDrivers {
    constexpr float P_MIN = -12.5;
    constexpr float P_MAX = 12.5;
    constexpr float V_MIN = -45.0;
    constexpr float V_MAX = 45.0;
    constexpr float T_MIN = -10.0;
    constexpr float T_MAX = 10.0;

    constexpr int BUFFER_SIZE = 1024 * 100;

        /* exact-width signed integer types */


    typedef   signed          char int8_t;
    typedef   signed short     int int16_t;
    typedef   signed           int int32_t;

        /* exact-width unsigned integer types */
    typedef unsigned          char uint8_t;
    typedef unsigned short     int uint16_t;
    typedef unsigned           int uint32_t;



    float uint_to_float(int x_int, float x_min, float x_max, int bits);
    int float_to_uint(float x, float x_min, float x_max, int bits);

    inline std::string getTimeStamp(){
        return std::to_string(std::chrono::duration_cast<std::chrono::milliseconds>
        (std::chrono::system_clock::now().time_since_epoch()).count());
    }

    class log{
    public:
        log(const std::string& logName, const std::string& logPath);
        void logWrite(const std::string& logContent);
        void logWriteTimestamp();
    private:
        std::string logName_;
        std::string logPath_;
        std::ofstream logFile_;
    };

    class SocketFrame{
    public:
        ~SocketFrame() = default;
        struct can_frame can_frame_;
        std::string timestamp_;
    };

    class HWSocket{
    public:
        virtual ~HWSocket() = default;
        virtual bool socket_Init() = 0;
        virtual bool socket_Send(uint8_t frame_data[]) = 0;
        virtual SocketFrame socket_Read() = 0;
        SocketFrame frame;
        std::deque<SocketFrame> data_buffer_;
        virtual int16_t socket_ReadMutation(uint8_t index) = 0;
        
    };

    class PID_Type
    {

    public:
        PID_Type(float kp_, float ki_, float kd_, float MaxOutput_, 
        float IntegralLimit_, float DeadBand_, float Max_Err_):
                kp(kp_), ki(ki_), kd(kd_), MaxOutput(MaxOutput_), 
                IntegralLimit(IntegralLimit_), DeadBand(DeadBand_), Max_Err(Max_Err_){};
        virtual ~PID_Type() = default;
        
        float target;				//目标值
        float kp;
        float ki;
        float kd;
        
        float measure;			//测量值
        float err;				//误差
        float last_err;      		//上次误差
        
        float pout;
        float iout;
        float dout;
        
        float output;				//本次输出
        float last_output;			//上次输出
        
        float MaxOutput;			//输出限幅
        float IntegralLimit;		//积分限幅
        float DeadBand;			    //死区（绝对值）
        float Max_Err;				//最大误差
        
        uint32_t thistime;
        uint32_t lasttime;
        uint8_t dtime;	

        float pidCalc(float _measure);

    private:
        void pidUpdatePeriod();
        log logPID = log("PID", "./logs/");
        void pidLog();
        
    };
    class Motor{
    
    public:
        explicit Motor(HWSocket* socket);
        virtual ~Motor() = default;

        void send_Empty();

        virtual bool motor_Init();
        virtual void motor_Update() = 0;
        virtual bool motor_getHWInfo() = 0;
        void motor_SetSocket(HWSocket* socket){socket_ = socket;}
        HWSocket* motor_GetSocket(){return socket_;}

        float targetPos_;
        float targetVel_;
        float crtPos_;
        float crtVel_;
        float crtTorque_;
        struct can_filter rfilter_[1];
        HWSocket* socket_;


    };

    

    class MotorDM : public Motor{
    public:
        using Motor::Motor;
        void motor_Enable();/// 使能帧 ///
        void motor_Disable();/// 失能帧 ///
        void motor_CleanPos();/// 位置标零 ///

        void motor_Update() override;/// 更新目标位置 ///
        bool motor_getHWInfo() override;/// 获取硬件信息 ///

    };

    class MotorM2006 : public Motor{
        constexpr static uint8_t SPEED_MODE = 0;
        constexpr static uint8_t POS_MODE = 1;
    public:
        using Motor::Motor;
        void motor_Update() override;
        bool motor_getHWInfo() override;
        void M2006Log();

        void motor_SetMode(uint8_t _mode){mode = _mode;}
        void motor_SetSpeedPID(float kp, float ki, float kd, 
        float MaxOutput, float IntegralLimit, float DeadBand);
        void motor_SetPosPID(float kp, float ki, float kd,
        float MaxOutput, float IntegralLimit, float DeadBand);
        inline void motor_SetPosTargetVel(float posTargetVel){posTargetVel_ = posTargetVel;}
        inline void motor_SetTargetPos(float targetPos){targetPos_ = targetPos;}
        inline void motor_SetTargetVel(float targetVel){targetVel_ = targetVel;}
        inline float motor_GetCrtPos() const{return crtPos_;}
        inline float motor_GetCrtVel() const{return crtVel_;}
        inline float motor_GetCrtTorque() const{return crtTorque_;}
        
    
    private:
        uint8_t mode = POS_MODE;
        PID_Type pid_speed = PID_Type(1.0, 0.1, 0.1, 300, 5000, 0.3, 100);
        PID_Type pid_pos = PID_Type(4.9, 0.02, 0.3, 300, 500, 0.3, 50);
        uint16_t p_int_prev;
        float crtOriginPos_;
        int16_t crtCycles_;
        int16_t crtOriginCycles_;
        log logM2006 = log("M2006", "./logs/");
        float posTargetVel_;
    };

    class MotorM3508 : public Motor{
        constexpr static uint8_t SPEED_MODE = 0;
        constexpr static uint8_t POS_MODE = 1;
    public:
        using Motor::Motor;
        void motor_Update() override;
        bool motor_getHWInfo() override;
        void M3508Log();

        void motor_SetMode(uint8_t _mode){mode = _mode;}
        void motor_SetSpeedPID(float kp, float ki, float kd, 
        float MaxOutput, float IntegralLimit, float DeadBand);
        void motor_SetPosPID(float kp, float ki, float kd,
        float MaxOutput, float IntegralLimit, float DeadBand);
        inline void motor_SetPosTargetVel(float posTargetVel){posTargetVel_ = posTargetVel;}
        inline void motor_SetTargetPos(float targetPos){targetPos_ = targetPos;}
        inline void motor_SetTargetVel(float targetVel){targetVel_ = targetVel;}
        inline float motor_GetCrtPos() const{return crtPos_;}
        inline float motor_GetCrtVel() const{return crtVel_;}
        inline float motor_GetCrtTorque() const{return crtTorque_;}

    private:
        uint8_t mode = POS_MODE;
        uint16_t p_int_prev;
        float crtOriginPos_;
        int16_t crtCycles_;
        int16_t crtOriginCycles_;
        PID_Type pid_speed = PID_Type(1.0, 0.1, 0.1, 6, 50, 0.3, 40);
        PID_Type pid_pos = PID_Type(4.9, 0.02, 0.3, 6, 50, 0.3, 40);
        log logM3508 = log("M3508", "./logs/");
        float posTargetVel_;
    };

    class M2006_Matrix{
    public:
        M2006_Matrix(std::vector<std::shared_ptr<MotorM2006>>* motors, HWSocket* s1, HWSocket* s2);
        ~M2006_Matrix() = default;
        
        
        bool motor_Init();
        void motor_Update();

    private:
        bool disabled[8];
        HWSocket* s1_;
        HWSocket* s2_;
        std::vector<std::shared_ptr<MotorDrivers::MotorM2006>>* motors_;
        
    };

    class M3508_Matrix{
    public:
        M3508_Matrix(std::vector<std::shared_ptr<MotorM3508>>* motors, HWSocket* s1, HWSocket* s2);
        ~M3508_Matrix() = default;
        
        


        bool motor_Init();
        void motor_Update();

    private:
        bool disabled[8];
        HWSocket* s1_;
        HWSocket* s2_;
        std::vector<std::shared_ptr<MotorDrivers::MotorM3508>>* motors_;
        
    };
    
}